#include <buola/mat.h>
#include <buola/mat/prettyprint.h>
#include <buola/mat/svd.h>
#include <buola/mat/traits_typename.h>
#include <buola/mat/eigen.h>

using namespace buola;

int main(int /*argc*/,char** argv)
{
/*
    start_timer();
    mat::CMat<long double> a=mat::random(lSize,lSize,std::normal_distribution<double>());
    mat::CMat<long double> b=mat::random(lSize,lSize,std::normal_distribution<double>());
    mat::CMat<long double> c=mat::random(lSize,lSize,std::normal_distribution<double>());
    mat::CMat<long double> d=mat::random(lSize,lSize,std::normal_distribution<double>());
    end_timer();
    start_timer();
    mat::CMat<long double> e=a*b*c*d;
    end_timer();
*/
/*    msg_info() << MTypeName<mat::CVec3d>::Get() << "\n";
    msg_info() << MTypeName<mat::CVec_d>::Get() << "\n";
    msg_info() << MTypeName<mat::CVecNd<7>>::Get() << "\n";

    mat::CMat_d a=mat::random(500,70,std::normal_distribution<double>());
    start_timer();
    mat::CMat_d b=pseudo_inverse(a);
    end_timer();
    mat::CMat_d f=b*a*b;
    msg_info() << norm(f-b) << "\n";
*/
/*
    mat::CMat3d lMat{1,1,1,1,1,1,1,1,1};
    mat::CVec3d lVec1{1,1,1};
    mat::CRow3d lVec2{1,1,1};
    
    msg_info() << lMat << "\n";
    msg_info() << lVec1*lVec2 << "\n";
    msg_info() << lMat+lVec1*lVec2 << "\n";
    msg_info() << lMat+lMat << "\n";
    msg_info() << lMat*lMat << "\n";
    msg_info() << lMat+lMat*lMat << "\n";
*/
///spline code!!!
/*
    std::vector<mat::CVec2d> lY{{1,1},{2,3},{3,3},{4,3},{5,4},{6,4},{6,3},{5,1},{4,-1},{3,-2},{2,-1},{1,-3},{0,-6}};
    int N=lY.size();
    mat::CVec_d lX=mat::linspace(N,0.0,(double)(N-1));
    
    mat::CMat_d lL=mat::zeros(N,3);
    mat::CVec_d lR=mat::zeros(N);
    
    //build tridiagonal matrix
    lL(0,1)=2.0/(lX[1]-lX[0]);
    lL(0,2)=1.0/(lX[1]-lX[0]);
    lR(0)=3.0*(lY[1][1]-lY[0][1])/sq(lX[1]-lX[0]);
    for(int i=1;i<N-1;i++)
    {
        lL(i,0)=1.0/(lX[i]-lX[i-1]);
        lL(i,1)=2*(1.0/(lX[i]-lX[i-1])+1.0/(lX[i+1]-lX[i]));
        lL(i,2)=1.0/(lX[i+1]-lX[i]);
        lR(i)=3.0*((lY[i][1]-lY[i-1][1])/sq(lX[i]-lX[i-1])+(lY[i+1][1]-lY[i][1])/sq(lX[i+1]-lX[i]));
    }
    lL(N-1,0)=1.0/(lX[N-1]-lX[N-2]);
    lL(N-1,1)=2.0/(lX[N-1]-lX[N-2]);
    lR(N-1)=3.0*(lY[N-1][1]-lY[N-2][1])/sq(lX[N-1]-lX[N-2]);
    
    //forward pass
    msg_info() << "before:" << lL << "\n";
    lL(0,2)=lL(0,2)/lL(0,1);
    lR(0)=lR(0)/lL(0,1);
    for(int i=1;i<N-1;i++)
    {
        lL(i,2)=lL(i,2)/(lL(i,1)-lL(i-1,2)*lL(i,0));
        lR(i)=(lR(i)-lR(i-1)*lL(i,0))/(lL(i,1)-lL(i-1,2)*lL(i,0));
    }
    
    mat::CVec_d lK(N);
    lK(N-1)=lR(N-1);
    for(int i=N-2;i>=0;i--)
    {
        lK(i)=lR(i)-lL(i,2)*lK(i+1);
    }
    
    msg_info() << "after:" << lL << "\n";
    msg_info() << "x:" << lK << "\n";

    std::vector<double> lXXV;
    std::vector<double> lYYV;
    for(int i=0;i<N-1;i++)
    {
        double y2=lY[i+1][1];
        double y1=lY[i][1];
        double a=lK(i)*(1)-(y2-y1);
        double b=-lK(i+1)*(1)+(y2-y1);
        for(int j=0;j<100;j++)
        {
            double t=j/100.0;
            lXXV.push_back(i+t);
            lYYV.push_back((1-t)*y1+t*y2+t*(1-t)*(a*(1-t)+b*t));
        }
    }
    msg_info() << "result:\n";
    for(int i=0;i<lXXV.size();i++)
    {
        msg_info() << lXXV[i] << " " << lYYV[i] << "\n";
    }
*/
/*
    mat::CMat3d lMat{10,0,0,0,10,0,0,0,10};
    mat::compute_roots(lMat);
    mat::CMat3d lMat2{26,-10.2,7,-10.2,51.73,-20.5,7,-20.5,14};
    mat::CMat3d lMat3=lMat2/max(abs(lMat2));
    mat::compute_roots(-lMat3);
    mat::compute_roots(lMat2);
*/
    mat::CVec3d lApproach(0,0,-1);
    mat::CVec3d lOriVector(-0.900969,0.433884,0);
    mat::CVec3d lThumbToFingers(1,0,0);
    mat::CQuat_d lRot(mat::CVec3d(0,0,1),lApproach);
    msg_info() << lRot << "\n" << (lRot*mat::CVec3d(0,0,1)).T() << (lRot*lThumbToFingers).T() << "\n";
    mat::CVec3d lRotFingers=lRot*lThumbToFingers;
    mat::CQuat_d lRoll(lRotFingers,lOriVector);
    mat::CQuat_d lRot2=lRoll*lRot*mat::CQuat_d(0,0,0,1);
    msg_info() << lRot2 << "\n";
}
